/*******************************************************************************
* This file implements the quaternion to calculate the attitude base on the
* data from the IMU.
*
* Author: Kong Wai Weng @ Cytron Technologies Sdn. Bhd.
*******************************************************************************/



#include "quat.h"



/*******************************************************************************
* PRIVATE CONSTANT DEFINITION                                                  *
*******************************************************************************/





/*******************************************************************************
* PRIVATE GLOBAL VARIABLES                                                     *
*******************************************************************************/

// Sensors reading.
static volatile float prv_fAccVector[3] = {0.0};
static volatile float prv_fGyroVector[3] = {0.0};
static volatile float prv_fMagVector[3] = {0.0};

// Quaternion.
static volatile float prv_fQuat[4] = {0.0};

// Direction Matrix.
static volatile float prv_fDcm[3][3] = {{0.0}};



/*******************************************************************************
* PRIVATE FUNCTION PROTOTYPES                                                  *
*******************************************************************************/

static void vNormalizeVector3(float fVector[3]);
static void vNormalizeVector4(float fVector[4]);
static void vQuatToMatrix(float fMatrix[3][3], float fQuat[4]);



/*******************************************************************************
* PUBLIC FUNCTION: vInitQuat
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Initialize the quaternion to the value sampled during start up.
* This function must be called after the sensors are initialized.
*
*******************************************************************************/
void vInitQuat(void)
{
	unsigned long lastUpdate;
	float rateSum[3];
	unsigned int i;

	prv_fAccVector[0] = 0.0f;
	prv_fAccVector[1] = 0.0f;
	prv_fAccVector[2] = -1.0f;
	vNormalizeVector3(prv_fAccVector);
	
	// Calculate the mag vector based on inclination.
	float x = cosf(MAG_INCL);
	float y = 0.0f;
	float z = -sinf(MAG_INCL);
	
	// Rotate local mag vector to align with true north.
	prv_fMagVector[0] = (x * cosf(MAG_DECL)) - (y * sinf(MAG_DECL));
	prv_fMagVector[1] = (y * cosf(MAG_DECL)) + (x * sinf(MAG_DECL));
	prv_fMagVector[2] = z;
	vNormalizeVector3(prv_fMagVector);
	
//    lastUpdate = AQ_LASTUPD;
//    while (AQ_LASTUPD == lastUpdate || AQ_RATEX == 0.0f)
//	;

	// Lazy way to initiate the quaternion to our current orientation.
	prv_fQuat[0] = 1.0f;
	prv_fQuat[1] = 0.0f;
	prv_fQuat[2] = 0.0f;
	prv_fQuat[3] = 0.0f;
	vQuatToMatrix(prv_fDcm, prv_fQuat);
	
//    configData.quatKp *= 20.0f;
//    for (i = 0; i < 2000; i++)
//	quatUpdate();
//    configData.quatKp /= 20.0f;
//
//    normalizeVector4(quatData.q);
//
//    // set the current bias to the readings we see now
//    rateSum[0] = 0.0f;
//    rateSum[1] = 0.0f;
//    rateSum[2] = 0.0f;
//    for (i = 0; i < 25; i++) {
//	while (AQ_LASTUPD == lastUpdate)
//	    ;
//	lastUpdate = AQ_LASTUPD;
//
//	rateSum[0] += AQ_RATEX;
//	rateSum[1] += AQ_RATEY;
//	rateSum[2] += AQ_RATEZ;
//    }
//    quatData.rateBias[0] = rateSum[0] / 25.0f;
//    quatData.rateBias[1] = rateSum[1] / 25.0f;
//    quatData.rateBias[2] = rateSum[2] / 25.0f;
//
//    quatData.loops = 0;
//
//    memset(quatStack, 0xcd, sizeof(quatStack));  // write known values into the stack
//    quatStack[0]=quatStack[1+STACKSIZE]=0xfacefeed; // put marker values at the words before/after the stack
//    ctl_task_run(&quatData.quatTask, 110, quatCode, 0, "quatTask", STACKSIZE, quatStack+1, 0);
}	



/*******************************************************************************
* PRIVATE FUNCTION: vQuatUpdate
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Initialize the quaternion to the value sampled during start up.
* This function must be called after the sensors are initialized.
*
*******************************************************************************/
static void vQuatUpdate(void)
{
	float fAcc[3], fMag[3];			// Measured acc & mag.
	float fEstAcc[3], fEstMag[3];	// Estimated acc & mag.
	float fRate[3];					// Angular velocity.
	float fRotError[3];				// Measured error.
	float fAccDev;					// Acc deviation from 1g.
	float fYaw, fPitch, fRoll;
	
	// Read from accelerometer, gyro and magnetometer.
	struct ACC_DATA xAccData = xReadAcc();
	struct GYRO_DATA xGyroData = xReadGyro();
	struct MAG_DATA xMagData = xReadMag();
	
	fAcc[0] = -xAccData.fAccX;
	fAcc[1] = -xAccData.fAccY;
	fAcc[2] = -xAccData.fAccZ;
	
	fMag[0] = xMagData.fMagX;
	fMag[1] = xMagData.fMagY;
	fMag[2] = xMagData.fMagZ;
	
//    // estimate bias shift due to motor speed (electromagnetic induced field)
//    gDev = 1.0f;
//    if (motorsData.throttle > 1000) {
//	float x, y, z;
//	float throt = (motorsData.throttle - 1000.0f) * adcData.vin;
//
//	x = mag[0] + configData.motorsMagBiasX * throt;
//	y = mag[1] + configData.motorsMagBiasY * throt;
//	z = mag[2] + configData.motorsMagBiasZ * throt;
//
//	mag[0] = x + x*configData.motorsMagScaleX*throt + y*configData.motorsMagAlgnXY*throt + z*configData.motorsMagAlgnXZ*throt;
//	mag[1] = y + x*configData.motorsMagAlgnYX*throt + y*configData.motorsMagScaleY*throt + z*configData.motorsMagAlgnYZ*throt;
//	mag[2] = z + x*configData.motorsMagAlgnZX*throt + y*configData.motorsMagAlgnZY*throt + z*configData.motorsMagScaleZ*throt;
	
//	// Calculate acc deviation from 1g.
//	fAccDev = 1.0f + (fabsf(sqrtf((fAcc[0] * fAcc[0]) + (fAcc[1] * fAcc[1]) + (fAcc[2] * fAcc[2])) - QUAT_GRAVITY) * configData.quatAccDist);
//	fAccDev = 1.0f / (fAccDev * fAccDev);
//    }
	
	vNormalizeVector3(fAcc);
	vNormalizeVector3(fMag);
	
	// Rotate gravity to body frame of reference.
	rotateVecByRevMatrix(fEstAcc, prv_fAccVector, prv_fDcm);
	
	// Rotate mags to body frame of reference.
	rotateVecByRevMatrix(fEstMag, prv_fMagVector, prv_fDcm);
	
//    // measured error, starting with accel vector
//    rotError[0] = -(acc[2] * estAcc[1] - estAcc[2] * acc[1]) * configData.quatKa * gDev;
//    rotError[1] = -(acc[0] * estAcc[2] - estAcc[0] * acc[2]) * configData.quatKa * gDev;
//    rotError[2] = -(acc[1] * estAcc[0] - estAcc[1] * acc[0]) * configData.quatKa * gDev;
//
//    // add in mag vector
//    rotError[0] += -(mag[2] * estMag[1] - estMag[2] * mag[1]) * configData.quatKm1;
//    rotError[1] += -(mag[0] * estMag[2] - estMag[0] * mag[2]) * configData.quatKm1;
//    rotError[2] += -(mag[1] * estMag[0] - estMag[1] * mag[0]) * configData.quatKm2;
//
//    // Integrate error into existing bias estimates
//    quatData.rateBias[0] -= rotError[0] * configData.quatKi;
//    quatData.rateBias[1] -= rotError[1] * configData.quatKi;
//    quatData.rateBias[2] -= rotError[2] * configData.quatKi;
//
//    // apply bias and P&I portions of error
//    dotRate[0] = (AQ_RATEX - quatData.rateBias[0]) * AQ_TIMESTEP + configData.quatKp * rotError[0];
//    dotRate[1] = (AQ_RATEY - quatData.rateBias[1]) * AQ_TIMESTEP + configData.quatKp * rotError[1];
//    dotRate[2] = (AQ_RATEZ - quatData.rateBias[2]) * AQ_TIMESTEP + configData.quatKp * rotError[2];
//
//    rotateQuat(quatData.q, quatData.q, dotRate);
//    quatToMatrix(quatData.m, quatData.q);
//
//    // extract and save euler angles
//    quatExtractEuler(quatData.q, &yaw, &pitch, &roll);
//    yaw = compassNormalize(yaw * RAD_TO_DEG);
//    pitch *= RAD_TO_DEG;
//    roll *= RAD_TO_DEG;
//
////    x' = x cos f - y sin f
////    y' = y cos f + x sin f
//    quatData.yawCos = cosf(yaw * DEG_TO_RAD);
//    quatData.yawSin = sinf(yaw * DEG_TO_RAD);
//    quatData.yaw = yaw;
//    quatData.pitch = pitch;
//    quatData.roll = roll;
//
//    quatData.lastUpdate = AQ_LASTUPD;
//    quatData.loops++;
}



/*******************************************************************************
* PRIVATE FUNCTION: vNormalizeVector3
*
* PARAMETERS:
* ~ fVector	- The vector that we want to normalize.
*			- Result will be saved to the same vector.
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Normalize the 3 dimensional vector.
*
*******************************************************************************/
static void vNormalizeVector3(float fVector[3])
{
    float fMagnitude;

    fMagnitude = sqrtf(	(fVector[0] * fVector[0]) +
    					(fVector[1] * fVector[1]) +
    					(fVector[2] * fVector[2])	);

    fVector[0] /= fMagnitude;
    fVector[1] /= fMagnitude;
    fVector[2] /= fMagnitude;
}



/*******************************************************************************
* PRIVATE FUNCTION: vNormalizeVector4
*
* PARAMETERS:
* ~ fVector	- The vector that we want to normalize.
*			- Result will be saved to the same vector.
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Normalize the 4 dimensional vector.
*
*******************************************************************************/
static void vNormalizeVector4(float fVector[4])
{
    float fMagnitude;

    fMagnitude = sqrtf(	(fVector[0] * fVector[0]) +
    					(fVector[1] * fVector[1]) +
    					(fVector[2] * fVector[2]) +
    					(fVector[3] * fVector[3])	);

    fVector[0] /= fMagnitude;
    fVector[1] /= fMagnitude;
    fVector[2] /= fMagnitude;
    fVector[3] /= fMagnitude;
}



/*******************************************************************************
* PRIVATE FUNCTION: vQuatToMatrix
*
* PARAMETERS:
* ~ fMatrix	- The result matrix.
* ~ fQuat	- The input quaternion.
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Change the quaternion into rotation matrix.
*
*******************************************************************************/
static void vQuatToMatrix(float fMatrix[3][3], float fQuat[4])
{
    float tmp1, tmp2;
    float sqw = fQuat[0] * fQuat[0];
    float sqx = fQuat[1] * fQuat[1];
    float sqy = fQuat[2] * fQuat[2];
    float sqz = fQuat[3] * fQuat[3];

    // get the invert square length
    //	float invs = 1.0f / (sqx + sqy + sqz + sqw);

    // rotation matrix is scaled by inverse square length
    fMatrix[0][0] = ( sqx - sqy - sqz + sqw); // * invs;
    fMatrix[1][1] = (-sqx + sqy - sqz + sqw); // * invs;
    fMatrix[2][2] = (-sqx - sqy + sqz + sqw); // * invs;

    tmp1 = fQuat[1] * fQuat[2];
    tmp2 = fQuat[3] * fQuat[0];
    fMatrix[1][0] = 2.0f * (tmp1 + tmp2); // * invs;
    fMatrix[0][1] = 2.0f * (tmp1 - tmp2); // * invs;

    tmp1 = fQuat[1] * fQuat[3];
    tmp2 = fQuat[2] * fQuat[0];
    fMatrix[2][0] = 2.0f * (tmp1 - tmp2); // * invs;
    fMatrix[0][2] = 2.0f * (tmp1 + tmp2); // * invs;

    tmp1 = fQuat[2] * fQuat[3];
    tmp2 = fQuat[1] * fQuat[0];
    fMatrix[2][1] = 2.0f * (tmp1 + tmp2); // * invs;
    fMatrix[1][2] = 2.0f * (tmp1 - tmp2); // * invs;
}



void rotateVecByMatrix(float *vr, float *v, float m[3][3]) {
    vr[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
    vr[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
    vr[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
}

void rotateVecByRevMatrix(float *vr, float *v, float m[3][3]) {
    vr[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
    vr[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
    vr[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
}

// no need to normalize as our quat stays very close to norm
void quatToMatrix(float m[3][3], float *q) {
    float tmp1, tmp2;
    float sqw = q[0]*q[0];
    float sqx = q[1]*q[1];
    float sqy = q[2]*q[2];
    float sqz = q[3]*q[3];

    // get the invert square length
    //	float invs = 1.0f / (sqx + sqy + sqz + sqw);

    // rotation matrix is scaled by inverse square length
    m[0][0] = ( sqx - sqy - sqz + sqw); // * invs;
    m[1][1] = (-sqx + sqy - sqz + sqw); // * invs;
    m[2][2] = (-sqx - sqy + sqz + sqw); // * invs;

    tmp1 = q[1]*q[2];
    tmp2 = q[3]*q[0];
    m[1][0] = 2.0f * (tmp1 + tmp2); // * invs;
    m[0][1] = 2.0f * (tmp1 - tmp2); // * invs;

    tmp1 = q[1]*q[3];
    tmp2 = q[2]*q[0];
    m[2][0] = 2.0f * (tmp1 - tmp2); // * invs;
    m[0][2] = 2.0f * (tmp1 + tmp2); // * invs;

    tmp1 = q[2]*q[3];
    tmp2 = q[1]*q[0];
    m[2][1] = 2.0f * (tmp1 + tmp2); // * invs;
    m[1][2] = 2.0f * (tmp1 - tmp2); // * invs;
}

void quatExtractEuler(float *q, float *yaw, float *pitch, float *roll) {
    float q0, q1, q2, q3;

    q0 = q[1];
    q1 = q[2];
    q2 = q[3];
    q3 = q[0];

    *yaw = atan2f((2.0f * (q0 * q1 + q3 * q2)), (q3*q3 - q2*q2 - q1*q1 + q0*q0));
    *pitch = asinf(-2.0f * (q0 * q2 - q1 * q3));
    *roll = atanf((2.0f * (q1 * q2 + q0 * q3)) / (q3*q3 + q2*q2 - q1*q1 -q0*q0));
}

// result and source can be the same
void rotateQuat(float *qr, float *q, float *rate) {
    float q1[4];
    float s, t, lg;
    float qMag;

    s = sqrtf(rate[0]*rate[0] + rate[1]*rate[1] + rate[2]*rate[2]) * 0.5f;
    t = -(0.5f * sinf(s) / s);
    rate[0] *= t;
    rate[1] *= t;
    rate[2] *= t;

    // create Lagrange factor to control quat's numerical integration errors
    qMag = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
    lg = cosf(s) + (1.0f - qMag*qMag) * DT * DT;

    // rotate
    q1[0] = q[0];
    q1[1] = q[1];
    q1[2] = q[2];
    q1[3] = q[3];

    qr[0] =  lg*q1[0]      + rate[0]*q1[1] + rate[1]*q1[2] + rate[2]*q1[3];
    qr[1] = -rate[0]*q1[0] + lg*q1[1]      - rate[2]*q1[2] + rate[1]*q1[3];
    qr[2] = -rate[1]*q1[0] + rate[2]*q1[1] + lg*q1[2]      - rate[0]*q1[3];
    qr[3] = -rate[2]*q1[0] - rate[1]*q1[1] + rate[0]*q1[2] + lg*q1[3];
}

void quatMatrixMultiply(float mr[3][3], float ma[3][3], float mb[3][3]) {
    mr[0][0] = ma[0][0]*mb[0][0] + ma[0][1]*mb[1][0] + ma[0][2]*mb[2][0];
    mr[0][1] = ma[0][0]*mb[0][1] + ma[0][1]*mb[1][1] + ma[0][2]*mb[2][1];
    mr[0][2] = ma[0][0]*mb[0][2] + ma[0][1]*mb[1][2] + ma[0][2]*mb[2][2];

    mr[1][0] = ma[1][0]*mb[0][0] + ma[1][1]*mb[1][0] + ma[1][2]*mb[2][0];
    mr[1][1] = ma[1][0]*mb[0][1] + ma[1][1]*mb[1][1] + ma[1][2]*mb[2][1];
    mr[1][2] = ma[1][0]*mb[0][2] + ma[1][1]*mb[1][2] + ma[1][2]*mb[2][2];

    mr[2][0] = ma[2][0]*mb[0][0] + ma[2][1]*mb[1][0] + ma[2][2]*mb[2][0];
    mr[2][1] = ma[2][0]*mb[0][1] + ma[2][1]*mb[1][1] + ma[2][2]*mb[2][1];
    mr[2][2] = ma[2][0]*mb[0][2] + ma[2][1]*mb[1][2] + ma[2][2]*mb[2][2];
}

void quatMatrixTranspose(float mt[3][3], float m[3][3]) {
    mt[0][0] = m[0][0];
    mt[0][1] = m[1][0];
    mt[0][2] = m[2][0];

    mt[1][0] = m[0][1];
    mt[1][1] = m[1][1];
    mt[1][2] = m[2][1];

    mt[2][0] = m[0][2];
    mt[2][1] = m[1][2];
    mt[2][2] = m[2][2];
}

